#include "StereoPicture.h"
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "BlobResult.h"
#include "opencv/cxcore.h"
#include <boost/function.hpp>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <time.h>
#include <math.h>

using namespace std;

int preFilterSize = 5;
int preFilterCap = 63;
int SADWindowSize = 15;
int minDisparity = 7;
int numberOfDisparities = 64;
int textureThreshold = 0;
int uniquenessRatio = 40;
int speckleWindowSize = 0;
int speckleRange = 0;

int ThresholdValue = 0;
int MinAreaValue = 500;
int MaxAreaValue = 25000;

IplImage* normalizedDisp = NULL;
CvMat *Q = NULL;
CvMat *depthM = NULL;

void OnpreFilterSize(int pos)
{

	int value = cvGetTrackbarPos("preFilterSize", "Stereo Controls");

	while (value % 2 == 0)
	{
		value += 1;
	}

	if(value < 5)
		value = 5;

	preFilterSize = value;
}

void OnpreFilterCap(int pos)
{
	preFilterCap = cvGetTrackbarPos("preFilterCap", "Stereo Controls");
}

void OnSADWindowSize(int pos)
{
	int value = cvGetTrackbarPos("SADWindowSize", "Stereo Controls");

	while (value % 2 == 0)
	{
		value += 1;
	}

	SADWindowSize = value;
}

void OnminDisparity(int pos)
{
	minDisparity = cvGetTrackbarPos("minDisparity", "Stereo Controls");
}

void OnnumberOfDisparities(int pos)
{
	int value = cvGetTrackbarPos("numberOfDisparities", "Stereo Controls");

	while(value % 16 != 0)
	{
		value += (16 - value%16);
	}

	numberOfDisparities = value;
}

void OntextureThreshold(int pos)
{
	textureThreshold = cvGetTrackbarPos("textureThreshold", "Stereo Controls");
}

void OnuniquenessRatio(int pos)
{
	uniquenessRatio = cvGetTrackbarPos("uniquenessRatio", "Stereo Controls");
}

void OnspeckleWindowSize(int pos)
{
	speckleWindowSize = cvGetTrackbarPos("speckleWindowSize", "Stereo Controls");
}

void OnspeckleRange(int pos)
{
	speckleRange = cvGetTrackbarPos("speckleRange", "Stereo Controls");
}

void OnThresholdValue(int pos)
{
	ThresholdValue = cvGetTrackbarPos("ThresholdValue", "Stereo Controls");
}

void OnMinAreaValue(int pos)
{
	MinAreaValue = cvGetTrackbarPos("MinAreaValue", "Stereo Controls");
}

void OnMaxAreaValue(int pos)
{
	MaxAreaValue = cvGetTrackbarPos("MaxAreaValue", "Stereo Controls");
}

void mouseHandler(int event, int x, int y, int flags, void *param)
{

	switch(event)
	{
		case CV_EVENT_LBUTTONDOWN:
		/*l = cvGet2D(stereoFunc->depthM, x, y);
		double CoordX,CoordY,CoordZ,W, d;

		printf("Clic at %d %d (%d %d)\n",x,y,normalizedDisp->width, normalizedDisp->height );

		d = cvGet2D(normalizedDisp, x, y).val[0];

		CoordX = x * Q[0, 0] + Q[0, 3];
		CoordY = y * Q[1, 1] + Q[1, 3];
		CoordZ = Q[2, 3];
		double W = d * Q[3, 2] + Q[3, 3];

		CoordX = x * cvGet2D(Q, 0, 0).val[0] + cvGet2D(Q,0, 3).val[0];
		CoordY = y * cvGet2D(Q, 1, 1).val[0] + cvGet2D(Q, 1, 3).val[0];
		CoordZ = cvGet2D(Q, 2, 3).val[0];
		W = d * cvGet2D(Q, 3, 2).val[0] + cvGet2D(Q, 3, 3).val[0];

		CoordX = CoordX / W;
		CoordY = CoordY / W;
		CoordZ = CoordZ / W;

		printf("Depth %f , %f, %f \n",CoordX,CoordY,CoordZ);*/

		printf("Distance to this object is: %f cm \n",(float)cvGet2D(depthM, x, y).val[0]);

		break;
	}
}


void Recons3D(CvMat *img_disparity, CvMat* vdisp, IplImage* &point3dPoll)
{
	//Get the interesting parameters from Q
	double Q03, Q13, Q23, Q32, Q33;
	Q03 = cvmGet(Q,0,3);
	Q13 = cvmGet(Q,1,3);
	Q23 = cvmGet(Q,2,3);
	Q32 = cvmGet(Q,3,2);
	Q33 = cvmGet(Q,3,3);

	double px, py, pz;

	for (int i = 0; i < vdisp->rows; i++)
	{

		for (int j = 0; j < vdisp->cols; j++)
		{
			if ( cvGet2D(vdisp,i,j).val[0] == 0 )
				continue; //Discard bad pixels

			uchar d =  cvGet2D(img_disparity,i,j).val[0];
			double pw = -1.0 * static_cast<double>(d) * Q32 + Q33;
			px = static_cast<double>(j) + Q03;
			py = static_cast<double>(i) + Q13;
			pz = Q23;

			CvScalar s = cvGet2D(point3dPoll,i,j);

			s.val[0] = px/pw;
			s.val[1] = py/pw;
			s.val[2] = pz/pw;

			cvSet2D(point3dPoll,i,j,s);
		}
	}
}

void MyRotate(CvPoint *ToRotate, CvPoint* RotationCenter, double RotationAngle)
{
	CvPoint pt = cvPoint (ToRotate->x - RotationCenter->x, ToRotate->y - RotationCenter->y);
	CvPoint pt2 = cvPoint (pt.x*cos(RotationAngle) - pt.y*sin(RotationAngle), pt.x*sin(RotationAngle) + pt.y*cos(RotationAngle));
	ToRotate->x = pt2.x + RotationCenter->x;
	ToRotate->y = pt2.y + RotationCenter->y;
}

double CalculateDepth(CvMat *_M1, CvMat* disp, CvMat* vdisp, int x, int y)
{
	if ((y >= vdisp->rows)|| x >= vdisp->cols)
		return 0.;

	double 	Depth = 0;

	double	focal = cvmGet(_M1,0,0);
	double	baseline = 5;
	double 	k1  = -0.004;
	double 	k2  = 0.51923;
	double 	k3  = -15.237;
	double 	k4  = 166.869;

	//Measure distance from depth map
	if(cvGet2D(vdisp,x,y).val[0]>0)
	{
		Depth = (double)((baseline*focal)/((double)(cvGet2D(disp,x,y).val[0]/16)));
		Depth = (k1*Depth*Depth*Depth + k2*Depth*Depth + k3*Depth + k4);
	}

	return Depth;
}

void ResetMap(IplImage** obstacleMap)
{
	CvPoint leftTopCorner = cvPoint(0,0);
	CvPoint rightBottomCorner = cvPoint((*obstacleMap)->width, (*obstacleMap)->height);
	cvRectangle(*obstacleMap, leftTopCorner, rightBottomCorner, CV_RGB(0,0,0), CV_FILLED);

	CvPoint pts1 = cvPoint(160, 0);
	CvPoint pts2 = cvPoint(160, 239);
	cvLine(*obstacleMap, pts1, pts2, CV_RGB(0,0,255));
}


void StereoPicture::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Func = boost::bind(&StereoPicture::IterationsThread, this);
	m_Thread = new boost::thread(Func);
}


// A Simple Camera Capture Framework
void StereoPicture::IterationsThread()
{

	Q = (CvMat *)cvLoad("Q.xml",NULL,NULL,NULL);
	CvMat *_M1 = (CvMat *)cvLoad("./Setup/M1.xml",NULL,NULL,NULL);
	CvMat *mx1 = (CvMat *)cvLoad("./Setup/mx1.xml",NULL,NULL,NULL);
	CvMat *my1 = (CvMat *)cvLoad("./Setup/my1.xml",NULL,NULL,NULL);
	CvMat *mx2 = (CvMat *)cvLoad("./Setup/mx2.xml",NULL,NULL,NULL);
	CvMat *my2 = (CvMat *)cvLoad("./Setup/my2.xml",NULL,NULL,NULL);

	// fps calculated using number of frames / seconds
	double fps;
	// frame counter
	int counter = 0;

	 // floating point seconds elapsed since start
	double sec;

	CvCapture* LeftCapture = cvCaptureFromCAM( 0 );
	cvSetCaptureProperty(LeftCapture,CV_CAP_PROP_FRAME_WIDTH, 320);
	cvSetCaptureProperty(LeftCapture,CV_CAP_PROP_FRAME_HEIGHT, 240);

	CvCapture* RightCapture = cvCaptureFromCAM( 1 );
	cvSetCaptureProperty(RightCapture,CV_CAP_PROP_FRAME_WIDTH, 320);
	cvSetCaptureProperty(RightCapture,CV_CAP_PROP_FRAME_HEIGHT, 240);

	if ( !LeftCapture || !RightCapture)
	{
		 fprintf( stderr, "ERROR: capture is NULL \n" );
		 getchar();
		 return;
	}

	// Create a window in which the captured images will be presented
	//cvNamedWindow( "Left", CV_WINDOW_AUTOSIZE );
	//cvNamedWindow( "Right", CV_WINDOW_AUTOSIZE );
	//cvNamedWindow( "Disparity", CV_WINDOW_AUTOSIZE );
	cvNamedWindow( "Obstacles Map", CV_WINDOW_AUTOSIZE );
	//cvNamedWindow( "Value", CV_WINDOW_AUTOSIZE );

	//cvSetMouseCallback( "Disparity", mouseHandler, NULL );
	 /*cvCreateTrackbar( "preFilterSize", "Value", &preFilterSize, 31, OnpreFilterSize );
	 cvCreateTrackbar( "preFilterCap", "Value", &preFilterCap, 120, OnpreFilterCap );
	 cvCreateTrackbar( "SADWindowSize", "Value", &SADWindowSize, 120, OnSADWindowSize );
	 cvCreateTrackbar( "minDisparity", "Value", &minDisparity, 120, OnminDisparity );
	 cvCreateTrackbar( "numberOfDisparities", "Value", &numberOfDisparities, 120, OnnumberOfDisparities );
	 cvCreateTrackbar( "textureThreshold", "Value", &textureThreshold, 120, OntextureThreshold );
	 cvCreateTrackbar( "uniquenessRatio", "Value", &uniquenessRatio, 120, OnuniquenessRatio );
	 cvCreateTrackbar( "speckleWindowSize", "Value", &speckleWindowSize, 120, OnspeckleWindowSize );
	 cvCreateTrackbar( "speckleRange", "Value", &speckleRange, 50000, OnspeckleRange );*/

	// Show the image captured from the camera in the window and repeat
	int i = 0, photoNum = 0;

	// Get one frame
	IplImage* Leftframe = cvQueryFrame( LeftCapture );
	IplImage* Rightframe = cvQueryFrame( RightCapture );

	IplImage *LeftGrayframe = cvCreateImage( cvSize( Leftframe->width, Leftframe->height ), IPL_DEPTH_8U, 1 );
	IplImage *RightGrayframe = cvCreateImage( cvSize( Rightframe->width, Rightframe->height ), IPL_DEPTH_8U, 1 );

	IplImage* LeftframeUndi = cvCreateImage( cvSize( Leftframe->width, Leftframe->height ), IPL_DEPTH_8U, 1 );
	IplImage* RightframeUndis = cvCreateImage( cvSize( Rightframe->width, Rightframe->height ), IPL_DEPTH_8U, 1 );

	depthM = cvCreateMat(Leftframe->height,  Leftframe->width, CV_32F);
	CvMat* disp = cvCreateMat( Leftframe->height,  Leftframe->width, CV_16S );
	CvMat* vdisp = cvCreateMat( Leftframe->height,  Leftframe->width, CV_8U );

	IplImage* normalizedBlobs = cvCreateImage( cvSize( Leftframe->width, Leftframe->height ), IPL_DEPTH_8U, 1 );

	IplImage* real_disparity = cvCreateImage( cvSize(Leftframe->width, Leftframe->height), IPL_DEPTH_8U, 1 );
	IplImage* point3dPoll = cvCreateImage( cvSize( Leftframe->width, Leftframe->height ), IPL_DEPTH_32F, 3 );

	IplImage* obstacleMap = cvCreateImage( cvSize( Leftframe->width, Leftframe->height ), IPL_DEPTH_8U, 1 );

	CvStereoBMState *BMState = cvCreateStereoBMState();

	CBlobResult LeftBlobs, RightBlobs;
	CBlob *currentBlob;
	CvPoint rect1, rect2, rect, rect_dist;

	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1, 1, 0, 1.4f, CV_AA);

	int balayageMin = 100;
	int balayageMil = 127;
	int balayageMax = 155;

	//Set rx pin in mode 0
	std::stringstream RxFileName;
	RxFileName<< "/sys/kernel/debug/omap_mux/ain2";
	int RxFd = open(RxFileName.str().c_str(), O_WRONLY);
	//write(RxFd , "20", 2);
	write(RxFd , "0x8000", 6);
	close(RxFd);

	std::stringstream FileName;
	FileName << "/sys/devices/platform/omap/tsc/ain2";

	int m_Ain1Fd = open(FileName.str().c_str(),O_RDONLY);

	servos->m_PanServo->m_Consigne = balayageMin;
	bool GoAtMin = false;

	boost::this_thread::sleep(boost::posix_time::milliseconds(2000));

	char buffer[1024];
	read(m_Ain1Fd, &buffer, sizeof(buffer));
	lseek(m_Ain1Fd, 0, 0);
	int balayageMinValue = atoi(buffer);

	servos->m_PanServo->m_Consigne = balayageMax;
	GoAtMin = true;

	boost::this_thread::sleep(boost::posix_time::milliseconds(2000));

	read(m_Ain1Fd, &buffer, sizeof(buffer));
	lseek(m_Ain1Fd, 0, 0);
	int balayageMaxValue = atoi(buffer);

	while (false == m_shallStop)
	{
		 read(m_Ain1Fd, &buffer, sizeof(buffer));
		 lseek(m_Ain1Fd, 0, 0);
		 int balayageCurValue = atoi(buffer);

		 if(true == GoAtMin)
		 {
			 if(balayageCurValue != balayageMaxValue)
				 continue;
		 }
		 else
		 {
			 if(balayageCurValue != balayageMinValue)
				 continue;
		 }

		 boost::this_thread::sleep(boost::posix_time::milliseconds(150));

		 // Get one frame
		 Leftframe = cvQueryFrame( LeftCapture );
		 Rightframe = cvQueryFrame( RightCapture );

		 read(m_Ain1Fd, &buffer, sizeof(buffer));

		 std::cout << "AIN1 VALUE : " << (atoi(buffer)*1800)/4095 << std::endl;

		 lseek(m_Ain1Fd, 0, 0);

		 if(true == GoAtMin)
		 {
			 servos->m_PanServo->m_Consigne = balayageMin;
		 }
		 else
		 {
			 servos->m_PanServo->m_Consigne = balayageMax;
		 }

		 cvCvtColor(Leftframe,LeftGrayframe,CV_RGB2GRAY);
		 cvCvtColor(Rightframe,RightGrayframe,CV_RGB2GRAY);

		 if ( !Leftframe || !Rightframe )
		 {
			   fprintf( stderr, "ERROR: frame is null...\n" );
			   getchar();
			   break;
		 }

		 cvRemap(LeftGrayframe, LeftframeUndi, mx1, my1);
		 cvRemap(RightGrayframe, RightframeUndis, mx2, my2);

		 BMState->preFilterSize = preFilterSize;
		 BMState->preFilterCap = preFilterCap;
		 BMState->SADWindowSize = SADWindowSize;
		 BMState->minDisparity = minDisparity;
		 BMState->numberOfDisparities = numberOfDisparities;
		 BMState->textureThreshold = textureThreshold;
		 BMState->uniquenessRatio = uniquenessRatio;
		 BMState->speckleWindowSize = speckleWindowSize;
		 BMState->speckleRange = speckleRange;

		 cvFindStereoCorrespondenceBM( LeftframeUndi, RightframeUndis, disp, BMState);

		 //Normalize the result so we can display it
		 cvNormalize( disp, vdisp, 0, 255, CV_MINMAX, NULL );

		 LeftBlobs = CBlobResult(cvGetImage(vdisp, normalizedBlobs), NULL, 0);

		 //Remove blobs if it does not cover minimum area specified below
		 LeftBlobs.Filter( LeftBlobs, B_EXCLUDE, CBlobGetArea(),B_OUTSIDE,MinAreaValue,MaxAreaValue);

		 // Color the blobs one after the other
		for (i = 0; i < LeftBlobs.GetNumBlobs(); i++ )
		{
			currentBlob = LeftBlobs.GetBlob(i);

			rect1.x = (int)currentBlob->MinX();
			rect1.y = (int)currentBlob->MinY();

			rect2.x = (int)currentBlob->MaxX();
			rect2.y = (int)currentBlob->MaxY();

			double Depth1, Depth2, Depth3, Depth4, Depth5;

			Depth1 = CalculateDepth(_M1, disp, vdisp, (rect1.y + rect2.y)/2, (rect1.x + rect2.x)/2);
			Depth2 = CalculateDepth(_M1, disp, vdisp, rect1.y + 2*(rect2.y - rect1.y)/3, rect1.x + (rect2.x - rect1.x)/3);
			Depth3 = CalculateDepth(_M1, disp, vdisp, rect1.y + 2*(rect2.y - rect1.y)/3, rect1.x + 2*(rect2.x - rect1.x)/3);
			Depth4 = CalculateDepth(_M1, disp, vdisp, rect1.y + (rect2.y - rect1.y)/3, rect1.x + (rect2.x - rect1.x)/3);
			Depth5 = CalculateDepth(_M1, disp, vdisp, rect1.y + (rect2.y - rect1.y)/3, rect1.x + 2*(rect2.x - rect1.x)/3);

			double depth = max(max(max(Depth1,Depth2),Depth3), max( Depth4, Depth5));

			if(depth > 100 || depth == 0)
				continue;

			int decalage = -0.302197 * depth + 38.7;

			CvPoint pt1 = cvPoint((int)currentBlob->MinX() - decalage, obstacleMap->height - (int)(depth * 2.4));
			CvPoint pt2 = cvPoint((int)currentBlob->MaxX() - decalage, obstacleMap->height - (int)(depth * 2.4));
			CvPoint pt3 = cvPoint(160, 239);

			if(true == GoAtMin)
			{
				MyRotate(&pt1, &pt3, M_PI/5);
				MyRotate(&pt2, &pt3, M_PI/5);
			}
			else
			{
				MyRotate(&pt1, &pt3, -M_PI/5);
				MyRotate(&pt2, &pt3, -M_PI/5);
			}

			cvLine(obstacleMap, pt1, pt2, CV_RGB(0,0,255),2);
			std::cout << "Obstacle " << i << " at " << depth << "cm." << std::endl;
		}

		cvShowImage( "Obstacles Map", obstacleMap );
		if(false == GoAtMin)
		{
			//cvShowImage( "Obstacles Map", Leftframe/*LeftframeUndi*/ );
			//cvShowImage( "Right", Rightframe/*RightframeUndis*/);
			//cvShowImage( "Obstacles Map", vdisp );
			//cvShowImage( "Obstacles Map", obstacleMap );
			ResetMap(&obstacleMap);
			//cvShowImage( "DetpthMap", point3dPoll );
		}

		 GoAtMin = !GoAtMin;
		//cvShowImage( "Obstacles Map", vdisp );
		//cvShowImage( "Obstacles Map", Leftframe/*LeftframeUndi*/ );

		 int KeyCode =  cvWaitKey(1);

		 // Do not release the frame!
		 //If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
		 //remove higher bits using AND operator
		 if ( (KeyCode & 255) == 27 )
			 break;
		 else if(KeyCode == 112) //P key
		 {
			 stringstream Name1, Name2;
			 Name1 << "Left" << photoNum << ".png";
			 Name2 << "Right" << photoNum << ".png";
			 cvSaveImage(Name1.str().c_str(), LeftGrayframe);
			 cvSaveImage(Name2.str().c_str(), RightGrayframe);
			 photoNum++;
		 }

	}

	// Release the capture device housekeeping
	cvReleaseCapture( &LeftCapture );
	cvReleaseCapture( &RightCapture );

	cvDestroyWindow( "Left" );
	cvDestroyWindow( "Right" );
	cvDestroyWindow( "Undistorded Left");
	cvDestroyWindow( "Undistorded Right");
	cvDestroyWindow( "Disparity");
	cvDestroyWindow( "Obstacles Map");
	return;
}

void StereoPicture::Stop()
{
	m_shallStop = true;
	if(NULL != m_Thread)
	{
		m_Thread->join();
		delete m_Thread;
	}
	std::cout << "Cams Stopped!" << std::endl;
}
